#include "CharacterController.h"

#include "InputsManager.h"

//////////////////////////////////////////////////////////////////////////////////////
// charcter controller
//////////////////////////////////////////////////////////////////////////////////////

void CharacterController::setXVelocity(float X)
{
	b2Vec2 vel = m_body->GetLinearVelocity();
	vel.x = X;
	m_body->SetLinearVelocity( vel );
}
void CharacterController::setYVelocity(float Y)
{
	b2Vec2 vel = m_body->GetLinearVelocity();
	vel.y = Y;
	m_body->SetLinearVelocity( vel );
}
void CharacterController::addXVelocity(float X)
{
	b2Vec2 vel = m_body->GetLinearVelocity();
	vel.x += X;
	m_body->SetLinearVelocity( vel );
}
void CharacterController::addYVelocity(float Y)
{
	b2Vec2 vel = m_body->GetLinearVelocity();
	vel.y += Y;
	m_body->SetLinearVelocity( vel );
}


//////////////////////////////////////////////////////////////////////////////////////
// BasicCharacterController
//////////////////////////////////////////////////////////////////////////////////////

BasicCharacterController::BasicCharacterController(  b2Body * _body )
		: CharacterController(_body)
{
	isJumping = false;
	AirAcc = 0.09375f;
	JumpVelocity = 6.5f;
	MinJumpVelocity = 4.0f;
	MinVelocityAirDrag = 0.125f;
	CoefAirDrag = 0.96875f;
	cmptTouchingGround = 0;

	maxXVelocity = 6.0f;
	maxYVelocity = 20.0f;
	XForce =  0.046875f;
	InvertXForce =  0.5f;
	XFriction = 0.046875f;
		
	gravity =  0.21875f;


	
	// register Sensor

	b2CircleShape circleSensor;
	b2FixtureDef Sensor;

	circleSensor.m_radius = 0.10f;

	circleSensor.m_p = b2Vec2( 0.25f, -0.5f /*+ circleSensor.m_radius*/ );
	Sensor.shape = &circleSensor;
	Sensor.isSensor = true;
	sensorFootRight = m_body->CreateFixture(&Sensor);

	circleSensor.m_p = b2Vec2( -0.25f, -0.5f /*+ circleSensor.m_radius*/ );
	Sensor.shape = &circleSensor;
	Sensor.isSensor = true;
	sensorFootLeft = m_body->CreateFixture(&Sensor);


	// Define another box shape for our dynamic body.
	b2PolygonShape rectSensor;
	rectSensor.SetAsBox(.1f, .40f, b2Vec2( -0.5f, 0.0f ), 0 );//These are mid points for our 1m box
	Sensor.shape = &rectSensor;
	Sensor.isSensor = true;
	sensorWallLeft = m_body->CreateFixture(&Sensor);

	rectSensor.SetAsBox(.1f, .40f, b2Vec2( 0.5f, 0.0f ), 0 );//These are mid points for our 1m box
	Sensor.shape = &rectSensor;
	Sensor.isSensor = true;
	sensorWallRight = m_body->CreateFixture(&Sensor);

	// end sensor
}

BasicCharacterController::~BasicCharacterController()
{
	m_body->DestroyFixture( sensorFootLeft );
	m_body->DestroyFixture( sensorFootRight );
	m_body->DestroyFixture( sensorWallLeft );
	m_body->DestroyFixture( sensorWallRight );
}

void BasicCharacterController::CheckSensor()
{
	sensorFootLeftIsColliding = false;
	sensorFootRightIsColliding = false;
	sensorWallLeftIsColliding = false;
	sensorWallRightIsColliding = false;

	b2ContactEdge* tmp = m_body->GetContactList();
	while( tmp )
	{
		if ( tmp->contact->GetFixtureA() == sensorFootLeft ||  tmp->contact->GetFixtureB() == sensorFootLeft )
		{
			sensorFootLeftIsColliding = tmp->contact->IsTouching();
		}

		if ( tmp->contact->GetFixtureA() == sensorFootRight ||  tmp->contact->GetFixtureB() == sensorFootRight )
		{
			sensorFootRightIsColliding = tmp->contact->IsTouching();
		}

		if ( tmp->contact->GetFixtureA() == sensorWallLeft ||  tmp->contact->GetFixtureB() == sensorWallLeft )
		{
			sensorWallLeftIsColliding = tmp->contact->IsTouching();
		}

		if ( tmp->contact->GetFixtureA() == sensorWallRight ||  tmp->contact->GetFixtureB() == sensorWallRight )
		{
			sensorWallRightIsColliding = tmp->contact->IsTouching();
		}

		tmp = tmp->next;
	}
}

bool BasicCharacterController::IsTouchingGround()
{
	return sensorFootLeftIsColliding || sensorFootRightIsColliding;
}

bool BasicCharacterController::IsTouchingWall()
{
	return ( sensorWallLeftIsColliding  )  || ( sensorWallRightIsColliding ) ;
}

bool BasicCharacterController::IsForcingTouchingWall()
{
	return ( 
			(sensorWallLeftIsColliding && 
					( INPUTS->GetKeyState( KEY_ID::KEY_LEFT ).Press || INPUTS->GetKeyState( KEY_ID::KEY_LEFT ).FrameSameState < 30 ) 
				)  
			|| 
			( sensorWallRightIsColliding && 
					( INPUTS->GetKeyState( KEY_ID::KEY_RIGHT ).Press || INPUTS->GetKeyState( KEY_ID::KEY_RIGHT ).FrameSameState < 30 ) 
				)
		);
}

void BasicCharacterController::draw( CCPoint Offset )
{
	

	// debug
	b2Fixture* tmp = sensorFootLeft ;

	if( sensorFootLeftIsColliding )
		glColor4f(1.0f, 0.0f, 0.0f, 1.0f );
	else
		glColor4f(0.0f, 1.0f, 0.0f, 1.0f );

	if( tmp->GetType() == b2Shape::e_circle )
	{
		ccDrawCircle( CCPoint( tmp->GetAABB().GetCenter().x *PTM_RATIO - Offset.x , tmp->GetAABB().GetCenter().y *PTM_RATIO - Offset.y ), tmp->GetAABB().GetExtents().x *PTM_RATIO, 0.0f, 20, false );
	}
	else
	{
		ccDrawLine( CCPoint( tmp->GetAABB().lowerBound.x *PTM_RATIO  - Offset.x, tmp->GetAABB().lowerBound.y *PTM_RATIO  - Offset.y ) ,
					CCPoint( tmp->GetAABB().upperBound.x *PTM_RATIO  - Offset.x, tmp->GetAABB().lowerBound.y *PTM_RATIO  - Offset.y )  );

		ccDrawLine( CCPoint( tmp->GetAABB().upperBound.x *PTM_RATIO  - Offset.x, tmp->GetAABB().lowerBound.y *PTM_RATIO  - Offset.y ) ,
					CCPoint( tmp->GetAABB().upperBound.x *PTM_RATIO  - Offset.x , tmp->GetAABB().upperBound.y *PTM_RATIO - Offset.y )  );

		ccDrawLine( CCPoint( tmp->GetAABB().upperBound.x *PTM_RATIO  - Offset.x, tmp->GetAABB().upperBound.y *PTM_RATIO  - Offset.y ) ,
					CCPoint( tmp->GetAABB().lowerBound.x *PTM_RATIO  - Offset.x, tmp->GetAABB().upperBound.y *PTM_RATIO  - Offset.y )  );

		ccDrawLine( CCPoint( tmp->GetAABB().lowerBound.x *PTM_RATIO  - Offset.x, tmp->GetAABB().upperBound.y *PTM_RATIO  - Offset.y ) ,
					CCPoint( tmp->GetAABB().lowerBound.x *PTM_RATIO  - Offset.x, tmp->GetAABB().lowerBound.y *PTM_RATIO  - Offset.y )  );
	}	


	// debug
	tmp = sensorFootRight ;

	if( sensorFootRightIsColliding )
		glColor4f(1.0f, 0.0f, 0.0f, 1.0f );
	else
		glColor4f(0.0f, 1.0f, 0.0f, 1.0f );


	if( tmp->GetType() == b2Shape::e_circle )
	{
		ccDrawCircle( CCPoint( tmp->GetAABB().GetCenter().x *PTM_RATIO - Offset.x , tmp->GetAABB().GetCenter().y *PTM_RATIO - Offset.y), tmp->GetAABB().GetExtents().x *PTM_RATIO, 0.0f, 20, false );
	}
	else
	{
		ccDrawLine( CCPoint( tmp->GetAABB().lowerBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().lowerBound.y *PTM_RATIO- Offset.y ) ,
					CCPoint( tmp->GetAABB().upperBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().lowerBound.y *PTM_RATIO- Offset.y )  );

		ccDrawLine( CCPoint( tmp->GetAABB().upperBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().lowerBound.y *PTM_RATIO- Offset.y ) ,
					CCPoint( tmp->GetAABB().upperBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().upperBound.y *PTM_RATIO- Offset.y )  );

		ccDrawLine( CCPoint( tmp->GetAABB().upperBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().upperBound.y *PTM_RATIO- Offset.y ) ,
					CCPoint( tmp->GetAABB().lowerBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().upperBound.y *PTM_RATIO- Offset.y )  );

		ccDrawLine( CCPoint( tmp->GetAABB().lowerBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().upperBound.y *PTM_RATIO- Offset.y ) ,
					CCPoint( tmp->GetAABB().lowerBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().lowerBound.y *PTM_RATIO- Offset.y )  );
	}	


	// debug
	tmp = sensorWallLeft ;

	if( sensorWallLeftIsColliding )
		glColor4f(1.0f, 0.0f, 0.0f, 1.0f );
	else
		glColor4f(0.0f, 1.0f, 0.0f, 1.0f );

	if( tmp->GetType() == b2Shape::e_circle )
	{
		ccDrawCircle( CCPoint( tmp->GetAABB().GetCenter().x *PTM_RATIO  , tmp->GetAABB().GetCenter().y *PTM_RATIO ), tmp->GetAABB().GetExtents().x *PTM_RATIO, 0.0f, 20, false );
	}
	else
	{
		ccDrawLine( CCPoint( tmp->GetAABB().lowerBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().lowerBound.y *PTM_RATIO- Offset.y ) ,
					CCPoint( tmp->GetAABB().upperBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().lowerBound.y *PTM_RATIO- Offset.y )  );

		ccDrawLine( CCPoint( tmp->GetAABB().upperBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().lowerBound.y *PTM_RATIO- Offset.y ) ,
					CCPoint( tmp->GetAABB().upperBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().upperBound.y *PTM_RATIO- Offset.y )  );

		ccDrawLine( CCPoint( tmp->GetAABB().upperBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().upperBound.y *PTM_RATIO- Offset.y ) ,
					CCPoint( tmp->GetAABB().lowerBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().upperBound.y *PTM_RATIO- Offset.y )  );

		ccDrawLine( CCPoint( tmp->GetAABB().lowerBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().upperBound.y *PTM_RATIO- Offset.y ) ,
					CCPoint( tmp->GetAABB().lowerBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().lowerBound.y *PTM_RATIO- Offset.y )  );
	}	


	// debug
	tmp = sensorWallRight ;

	if( sensorWallRightIsColliding )
		glColor4f(1.0f, 0.0f, 0.0f, 1.0f );
	else
		glColor4f(0.0f, 1.0f, 0.0f, 1.0f );

	if( tmp->GetType() == b2Shape::e_circle )
	{
		ccDrawCircle( CCPoint( tmp->GetAABB().GetCenter().x *PTM_RATIO  , tmp->GetAABB().GetCenter().y *PTM_RATIO ), tmp->GetAABB().GetExtents().x *PTM_RATIO, 0.0f, 20, false );
	}
	else
	{
		ccDrawLine( CCPoint( tmp->GetAABB().lowerBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().lowerBound.y *PTM_RATIO- Offset.y ) ,
					CCPoint( tmp->GetAABB().upperBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().lowerBound.y *PTM_RATIO- Offset.y )  );

		ccDrawLine( CCPoint( tmp->GetAABB().upperBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().lowerBound.y *PTM_RATIO- Offset.y ) ,
					CCPoint( tmp->GetAABB().upperBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().upperBound.y *PTM_RATIO- Offset.y )  );

		ccDrawLine( CCPoint( tmp->GetAABB().upperBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().upperBound.y *PTM_RATIO- Offset.y ) ,
					CCPoint( tmp->GetAABB().lowerBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().upperBound.y *PTM_RATIO- Offset.y )  );

		ccDrawLine( CCPoint( tmp->GetAABB().lowerBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().upperBound.y *PTM_RATIO- Offset.y ) ,
					CCPoint( tmp->GetAABB().lowerBound.x *PTM_RATIO - Offset.x , tmp->GetAABB().lowerBound.y *PTM_RATIO- Offset.y )  );
	}	


	glColor4f(1.0f, 1.0f, 1.0f, 1.0f );
}

void BasicCharacterController::update(ccTime dt)
{
	CheckSensor();

	// debug loop in a screen
	/*
	if( m_body->GetPosition().x > 400.0f/PTM_RATIO )
	{
		b2Vec2 pos =  m_body->GetPosition();
		pos.x = 20.0f/PTM_RATIO;
		m_body->SetTransform( pos , 0.0f );
	}
	else if( m_body->GetPosition().x < 20.0f/PTM_RATIO )
	{
		b2Vec2 pos =  m_body->GetPosition();
		pos.x = 400.0f/PTM_RATIO;
		m_body->SetTransform( pos , 0.0f );
	}
	*/
	// touching ground 
	if( m_body->GetLinearVelocity().y == 0.0f )
	{
		if( cmptTouchingGround > 0 )
		{
			cmptTouchingGround --;
		}
		else
		{
			isJumping = false;
		}
	}

	// gravity
	addYVelocity( -gravity );
	
	// max Y speed
	if( abs( m_body->GetLinearVelocity().y ) > maxYVelocity ) 
	{
		float sign = m_body->GetLinearVelocity().y >= 0.0f ? 1.0f : -1.0f;
		setYVelocity(maxYVelocity * sign);
	}

	// inputs 

	// Horizontal 

	if ( INPUTS->GetKeyState( KEY_ID::KEY_RIGHT ).Press )
	{
		if( m_body->GetLinearVelocity().x < 0.0f )
		{
			// decc
			addXVelocity( InvertXForce );
		}
		else if( m_body->GetLinearVelocity().x < maxXVelocity )
		{
			// acc
			if( ! IsTouchingGround() )
				addXVelocity( AirAcc );
			else
				addXVelocity( XForce );


			// max speed
			if( m_body->GetLinearVelocity().x > maxXVelocity ) 
				setXVelocity(maxXVelocity);
		}
	}
	else if ( INPUTS->GetKeyState( KEY_ID::KEY_LEFT ).Press )
	{
		if( m_body->GetLinearVelocity().x > 0.0f )
		{
			// dec
			addXVelocity( -InvertXForce );
		}
		else if( m_body->GetLinearVelocity().x > -maxXVelocity )
		{
			// acc
			if( ! IsTouchingGround() )
				addXVelocity( -AirAcc );
			else
				addXVelocity( -XForce );

			// max speed
			if( m_body->GetLinearVelocity().x < -maxXVelocity ) 
				setXVelocity(-maxXVelocity);
		}
	}
	else
	{
		// friction
		if( ! IsTouchingGround() )
		{
			if( m_body->GetLinearVelocity().y > 0 && m_body->GetLinearVelocity().y < MinJumpVelocity )
			{
				if( abs(m_body->GetLinearVelocity().x) >= MinVelocityAirDrag )
				{
					setXVelocity( m_body->GetLinearVelocity().x * CoefAirDrag );
				}
			}
		}
		else
		{
			float sign = m_body->GetLinearVelocity().x >= 0.0f ? 1.0f : -1.0f;
			addXVelocity( (min( abs(m_body->GetLinearVelocity().x) , XFriction ) * sign) * -1 );
		}
	}
	

	// Vertical
	if ( INPUTS->GetKeyState( KEY_ID::KEY_JUMP ).Pressed )
	{
		if( IsTouchingGround() )
		{
			setYVelocity( JumpVelocity );
			isJumping = true;
			cmptTouchingGround = 3;
		}
	}
	else if ( INPUTS->GetKeyState( KEY_ID::KEY_JUMP ).Press == false )
	{
		if( IsTouchingGround() == false && m_body->GetLinearVelocity().y > 0 )
		{
			if(  m_body->GetLinearVelocity().y > MinJumpVelocity )
			{
				setYVelocity( MinJumpVelocity );
			}
		}
	}
}



